package stephencarmody.k8.physics;

import stephencarmody.k8.core.Log;
import stephencarmody.k8.core.Logic;

public final class State extends Logic {

	public float x;
	public float v;
	
	// RK4 derivatives
	private static float da_x, da_v;
	private static float db_x, db_v;
	private static float dc_x, dc_v;
	private static float dd_x, dd_v;
	private static float dxdt, dvdt;
	
	// timestep
	private static long lasttime;
	private static float dt;
	
	// Constants for spring and damper force
	private static final float k = 10;
	private static final float b = 1;
	
	public State(float x, float v) {
		this.x = x;
		this.v = v;
		setUpdateRate(10);
	}
	
	/**
	 * State is "updated" using the Runge Kutta order 4 integration aka RK4. This is the
	 * standard integrator used for numerical integration these days and is sufficiently
	 * accurate for just about anything required in game physics, given an appropriate
	 * timestep.
	 * 
	 * An RK4 integrator works by evaluating the derivatives at several points in the
	 * timestep to detect this curvature, then it combines these sampled derivatives with
	 * a weighted average to get the single best approximation of the derivative that it
	 * can provide over the timestep.
	 */
	@Override
	public void update(long time) {
		// Calculate elapsed time in seconds
		dt = ((float)(time - lasttime)/1000000000);
		
		Log.info("x:" + x + ", v:" + v);
		Log.info("dt:" + dt);
		
		// Calculate first derivative
		da_x = v;
		da_v = -k*x - b*v;
		
		Log.info("da_x:" + da_x + ", da_v:" + da_v);
		
		// Calculate second derivative
		db_x = v + da_v*dt*0.5f;
		db_v = -k*(x+da_x*dt*0.5f) - b*(v+da_v*dt*0.5f);
		
		Log.info("db_x:" + db_x + ", db_v:" + db_v);
		
		// Calculate third derivative
		dc_x = v + db_v*dt*0.5f;
		dc_v = -k*(x+db_x*dt*0.5f) - b*(v+db_v*dt*0.5f);

		Log.info("dc_x:" + dc_x + ", dc_v:" + dc_v);
		
		// Calculate fourth derivative
		dd_x = v + dc_v*dt;
		dd_v = -k*(x+dc_x*dt) - b*(v+dc_v*dt);
		
		Log.info("dd_x:" + dd_x + ", dd_v:" + dd_v);
		
		// Calculate the best overall derivative using a weighted sum of the derivatives
		dxdt = (da_x + 2*(db_x + dc_x) + dd_x)/6f;
		dvdt = (da_v + 2.0f*(db_v + dc_v) + dd_v)/6f;
		
		Log.info("dxdt:" + dxdt + ", dvdt:" + dvdt);
		
		// Advance the position and velocity over dt
		x += dxdt*dt;
		v += dvdt*dt;
		
		// Remember current timestamp
		lasttime = time;
		
		Log.info("x:" + x + ", v:" + v);
	}
	
}
